from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    """
    用于启动一个带有控制器的机器人和仿真环境
    """

    controller = Node(
        package="controller",
        executable="tri_pid",
        name="controller",
        output="screen",
        parameters=[
            {"gyro_pid": [0.1, 0.5, 0.0, 100.0]},
            {"angle_pid": [5.0, 0.0, 0.0, 100.0]},
            {"speed_pid": [0.0, 0.0, 0.0, 100.0]},
            {"rotate_pid": [0.2, 1.0, 0.0, 100.0]},
            {"state_rate": 100.0},
        ],
    )

    webotsSim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [FindPackageShare("robot_sim"), "launch", "sim_direct.launch.py"]
            )
        )
    )

    return LaunchDescription(
        [
            webotsSim,
            controller,
        ]
    )
